#include "include/data_processor.h"
// 构造函数
// DLOG_MODULE_DEFINE(NodeCamdar1)

DataProcessor::DataProcessor()
{
    // stateManager_ = &StateConfigManager::getInstance(); // 在构造函数体内赋值
    // cfg_ = gscm.getConfig();

    startProcessingThread();
    LOG_CRITICAL("DATA PROCESS INIT SUCESSS!");
    Init();

}

void DataProcessor::Init()
{
    data_fusion_ = std::make_shared<DataFusion>();
    // cfg_ = gscm.getConfig();
}

void DataProcessor::startProcessingThread() 
{
    processing_thread_ = std::thread(&DataProcessor::processMessages, this);
}

void DataProcessor::processMessages()
{
    while (true) 
    {
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
        bool bLogic = gscm.getLogic();
        bool bSwitch = gscm.getSwitch();
        if(bLogic == true)
        {
            // int sensor = stateManager_->getSensor();
            int sensor = 0;
            if(bSwitch == false)
            {
                std::cout << "sensor: " << static_cast<int>(SENSOR::ALL) << std::endl;
                if (sensor == static_cast<int>(SENSOR::ALL))
                {
                    multiSenosrs();
                }
                else if (sensor == static_cast<int>(SENSOR::LIDAR))
                {
                    /* code */
                    LidarSensor();
                }
                else if(sensor == static_cast<int>(SENSOR::INVALID))
                {

                }
                  
            }

            else if(bSwitch == true)
            {
                LOG_CRITICAL("Start sensor turn  Switch: {}, sensor: {}", bSwitch, sensor);
                bSwitch = false;
            }

        }
    }
}


bool DataProcessor::matchingSteps(int type, int& status,  msg::ObstacleData& cur_lidar_data, msg::AiMetadata& cur_ai_data)
{
    // msg::ObstacleData& closest_lidar_data;
    int ai_len = ai_data_deque_.size();
    int lidar_len = lidar_data_deque_.size();

    if(ai_len == 0 && lidar_len == 0)
    {
        status = -1;
        // std::cout << "case1\n";
        return false;
    }
    else if(ai_len == 0 && lidar_len != 0)
    {
        if(lidar_len == 1)
        {
            // wait
            status = 0;
            // std::cout << "case2\n";

            return  false;
        }
        else if(lidar_len == 2)
        {
            // std::cout << "case3" << std::endl;
            status = 1;
            auto t1 = lidar_data_deque_[0]->time_stamp;
            auto t2 = lidar_data_deque_[1]->time_stamp;
            int64_t LIDAR_INTERVAL = cfg_.time_interval.lidar_interval;
            if(t2 - t1 < 1.5 * LIDAR_INTERVAL)
            {
                // lidar_data_deque_.pop_front();
                return false;
            }
            else
            {
                cur_lidar_data = *lidar_data_deque_.back();
                lidar_data_deque_.pop_front();
                return true;
            }

        }
        else
        {
            // std::cout << "case4" << std::endl;
            if(lidar_data_deque_.size() >= 15)
            {
                status = 2;
                // std::cout <<  "ERRROR: LIDAR size() >= 3:\n";
                cur_lidar_data = *lidar_data_deque_.back();
                lidar_data_deque_.pop_front();
                return true;
            }
            return false;
        }

    }
    else if(ai_len != 0 && lidar_len == 0)
    {
        // std::cout << "case5" << std::endl;

        if(ai_len == 1)
        {
            status = 11;
            return false;
        }
        else if(ai_len == 2)
        {
            status = 12;
            auto t1 = ai_data_deque_[0]->time_stamp;
            auto t2 = ai_data_deque_[1]->time_stamp;
            int64_t AI_INTERVAL = cfg_.time_interval.ai_interval;

            if(t2 - t1 < 1.5 * AI_INTERVAL)
            {
                // lidar_data_deque_.pop_front();
                return false;
            }
            else
            {
                if (buse_frames)
                {
                    cur_ai_data = *ai_data_deque_.back();
                    ai_data_deque_.pop_front();
                    return true;
                }
                return true;
            }
        }
        else
        {
            // std::cout << "case6" << std::endl;

            status = 22;
            // std::cout <<  "ERRROR: AI size() >= 3:\n";
            cur_ai_data = *ai_data_deque_.back();
            ai_data_deque_.pop_back();
            return true;
        }

    }
    else
    {
        // std::cout << "case7" << std::endl;

        status = 31;
        msg::ObstacleData closest_lidar_data;
        msg::AiMetadata  closet_ai_data;
        if(findClosestLidarData(closet_ai_data, closest_lidar_data))
        {
            cur_ai_data = closet_ai_data;
            cur_lidar_data = closest_lidar_data;
            return true;
        }
        else
        {
            return false;
        }
    }

    LOG_I("Satatus {}", status);
} 


void DataProcessor::handleData(const std::shared_ptr<void>& data, DataType type)
{
    switch (type) 
    {
        case DataType::Lidar: {
            // 将 void* 转换为具体类型
            auto lidar_data = std::static_pointer_cast<msg::ObstacleData>(data);
            if (lidar_data) {
                SubLidarObstacleData(lidar_data);
            } else {
                std::cerr << "Error: Invalid Lidar data type" << std::endl;
            }
            break;
        }
        case DataType::AI: {
            // 将 void* 转换为具体类型
            auto ai_data = std::static_pointer_cast<msg::AiMetadata>(data);
            if (ai_data) {
                // processAiData(ai_data);
                SubAiMetadata(ai_data);
            } else {
                std::cerr << "Error: Invalid AI data type" << std::endl;
            }
            break;
        }
        case DataType::APP: {
            auto app_data = std::static_pointer_cast<msg::AppCamdar>(data);
            if (app_data) {
                // processAiData(app_data);
                processAppData(app_data);
            } else {
                std::cerr << "Error: Invalid AI data type" << std::endl;
            }
            break;
        }
        default:
            std::cerr << "Error: Unsupported data type" << std::endl;
            break;
    }
}

void DataProcessor::SubLidarObstacleData(const msg::ObstacleData::SharedPtr& data) 
{
    int SENSOR_FUNCTION = cfg_.module_function.sensor;
    if(gscm.getLogic() &&  (SENSOR_FUNCTION == 0 || SENSOR_FUNCTION == 2))
    {
        bool DEBUG_INFO_LIDAR = cfg_.debug_info.debug_lidar;
        if(DEBUG_INFO_LIDAR)
        {
            std::cout << "sub lidar info ---------------------" << std::endl;
            std::cout << "lidar timestamp: " << data->time_stamp << std::endl;
            std::cout << "lidar size(): " << data->obs.size() << std::endl;
            std::cout << "end lidar info ---------------------" << std::endl;
        }

        {
            gscm.updateLiPreTime();
            std::lock_guard<std::mutex> lock(lidar_deque_mutex_);
            lidar_data_deque_.push_back(data);
            last_data_type_ = 2;
            start_matching_ = true;
        }

        data_cond_.notify_one();
    }
}


// 处理 AI 数据
void DataProcessor::SubAiMetadata(const msg::AiMetadata::SharedPtr& data) 
{
    int SENSOR_FUNCTION = cfg_.module_function.sensor;

    if(gscm.getLogic() && SENSOR_FUNCTION == 0)
    {
        bool DEBUG_INFO_AI = cfg_.debug_info.debug_ai;
        if(DEBUG_INFO_AI)
        {
            // auto ai_local_time = getLocalMachineTime();
            std::cout << "sub ai info ---------------------" << std::endl;
            std::cout << "Ai time_stamp: " << data->time_stamp << std::endl;
            // std::cout << "local time_stamp: " << ai_local_time << std::endl;
            std::cout << "Ai->bndy.time_stamp: " << data->bndy.time_stamp << std::endl;
            // uint8 print mode
            printf("Ai->bndy.visionfield: %d\n", data->bndy.visionfield);
            printf("Ai->bndy.bndygrass2boundary: %d\n", data->bndy.grass2boundary);
            printf("Ai->bndy.boardhasobs: %d\n", data->bndy.boardhasobs);
            printf("Ai->bndy.bndyboudarynum: %d\n", data->bndy.boudarynum);
            std::cout << "Ai->bndy.boudary.size(): " << data->bndy.boundary.size() << std::endl;
            if (data->bndy.boundary.size() > 0)
            {
                std::cout << "xyz: " << 
                    data->bndy.boundary[0].x << " " << 
                    data->bndy.boundary[0].y << " " << 
                    data->bndy.boundary[0].z << std::endl;
            }

            std::cout << "Ai->det.time_stamp: " << data->det.time_stamp << std::endl;
            std::cout << "Ai->det.bboxes.size(): " << data->det.bboxes.size() << std::endl;
            std::cout << "Ai->det.mask_w: " << data->det.mask_w  <<  std::endl;
            std::cout << "Ai->det.mask_h: " << data->det.mask_h  <<  std::endl;
            std::cout << "Ai->det.mask_size() : " << data->det.mask.size() / sizeof(uint8_t)  <<  std::endl;
            std::cout << "end ai info ---------------------" << std::endl;
        }
        {
            gscm.updateAiPreTime();
            std::lock_guard<std::mutex> lock(ai_deque_mutex_);
            data->det.time_stamp = data->det.time_stamp  - cfg_.time_interval.ai_delay;
            data->time_stamp = data->time_stamp - cfg_.time_interval.ai_delay;
            ai_data_deque_.push_back(data);
            last_data_type_ = 1;
            start_matching_ = true;
        }

        data_cond_.notify_one();
    }

}



void DataProcessor::processAppData(const msg::AppCamdar::SharedPtr& data)
{
    if(data == nullptr)
        return ;
    int status = data->status;
    int sensor = data->sensor_module;
    float lower_altitude = data->lower_altitude;
    // LOG_CRITICAL("receive  STATUS:{} {} {} ", status, sensor, lower_altitude);
    LOG_CRITICAL("receive  STATUS:{} {} {} ", status, sensor, lower_altitude);
    gscm.setAppData(status, sensor, lower_altitude);
}



void DataProcessor::LidarSensor()
{
    std::unique_lock<std::mutex> li_lock(lidar_deque_mutex_);

    data_cond_.wait(li_lock, [this] { 
        std::lock_guard<std::mutex> ai_guard(lidar_deque_mutex_, std::adopt_lock);
        return start_matching_ && !lidar_data_deque_.empty(); 
    });
    // LOG_CRITICAL("bSwitch: {}, sensor: {}", bSwitch, SENSOR_FUNCTION);

    msg::AiMetadata cur_ai_data;
    msg::ObstacleData cur_li_data;
    {
        start_matching_ = false;
        cur_li_data = *lidar_data_deque_.front();
        lidar_data_deque_.pop_front();
        auto ai_msg_ptr = std::make_shared<common::msg::AiMetadata>(cur_ai_data);
        auto li_msg_ptr = std::make_shared<common::msg::ObstacleData>(cur_li_data);
        
        // LOG_CRITICAL("tmp2 print sensor: {} high: {}", SENSOR_FUNCTION, LOWER_AIMS_ATTITUDE);
        // CamdarDataFusion2(li_msg_ptr, ai_msg_ptr);

        

    }
}

void DataProcessor::multiSenosrs()
{
    std::unique_lock<std::mutex> ai_lock(ai_deque_mutex_);
    std::unique_lock<std::mutex> lidar_lock(lidar_deque_mutex_);

    // std::cout << start_matching_ << std::endl;
    // std::cout << ai_data_deque_.size() << std::endl;
    // std::cout << lidar_data_deque_.size() << std::endl;
    data_cond_.wait(ai_lock, [this] { 
        std::lock_guard<std::mutex> lidar_guard(lidar_deque_mutex_, std::adopt_lock);
        return start_matching_ && (!ai_data_deque_.empty() || !lidar_data_deque_.empty()) ; 
    });

    // LOG_CRITICAL("bSwitch: {}", bSwitch);
    int status = -1;
    msg::ObstacleData cur_lidar_data;
    msg::AiMetadata cur_ai_data;
    bool bMatch = false;
    if (last_data_type_ == 1)
    {
        start_matching_ = false;
        bMatch = matchingSteps(last_data_type_, status, cur_lidar_data, cur_ai_data);

    }
    else if (last_data_type_ == 2)
    {
        start_matching_ = false;
        bMatch = matchingSteps(last_data_type_, status, cur_lidar_data, cur_ai_data);
    }
    else
    {
        ;
    }
    
    
    if(status == 31 && bMatch)
    {
        auto lidar_msg_ptr = std::make_shared<common::msg::ObstacleData>(cur_lidar_data);
        auto ai_msg_ptr = std::make_shared<common::msg::AiMetadata>(cur_ai_data);
        // if(MODULE_FUNCTION == 0)
        {
            // LOG_CRITICAL("tmp print sensor: {} high: {}", SENSOR_FUNCTION, LOWER_AIMS_ATTITUDE);
            
            {
                std::cout << "bmath: " << bMatch << std::endl;
                std::cout << "lidar: " <<  lidar_msg_ptr->time_stamp << std::endl;
                std::cout <<  "ai  : " <<  ai_msg_ptr->time_stamp << std::endl;
                // CamdarDataFusion(lidar_msg_ptr, ai_msg_ptr);
                data_fusion_->update(lidar_msg_ptr, ai_msg_ptr);
            }

        }

    }



}

bool DataProcessor::findClosestLidarData(msg::AiMetadata& closest_ai_data, msg::ObstacleData& closest_lidar_data)
{
    // std::lock_guard<std::mutex> lock_lidar(lidar_deque_mutex_);
    // std::lock_guard<std::mutex> lock_ai(ai_deque_mutex_);

    auto ai_len = ai_data_deque_.size();
    auto lidar_len = lidar_data_deque_.size();
    long long min_diff = std::numeric_limits<long long>::max();
    bool found = false;
    std::vector<float> time_interval;
    int sensor_index = 0;
    int sensor_type = 0;
    int64_t TIME_DIFF = cfg_.time_interval.time_diff;

    if(lidar_len >= ai_len)
    {

        sensor_type = 1;
        auto cur_ai_data_ptr = ai_data_deque_.back();
        for (auto it = lidar_data_deque_.rbegin(); it != lidar_data_deque_.rend(); ++it) 
        {
            auto lidar_data = *it;
            sensor_index += 1;
            long long diff = std::llabs(cur_ai_data_ptr->time_stamp - lidar_data->time_stamp);
            if (diff <= min_diff) 
            {
                min_diff = diff;
                LOG_CRITICAL("time interval diff  {} {} {} ", min_diff, cur_ai_data_ptr->time_stamp, lidar_data->time_stamp);
                if (diff <= TIME_DIFF) 
                {
                    closest_lidar_data = *lidar_data;
                    closest_ai_data = *cur_ai_data_ptr; 
                    found = true;
                }
                else
                {
                    // std::cout << "LIDAR BAD ERROE TIME INTERVAL IS BIG\n"; 
                    time_interval.push_back(diff);
                    found = false;
                    // return false;
                }
            }
        }

    }
    else
    {
        sensor_type = 2;
        auto cur_lidar_data_ptr = lidar_data_deque_.back();
        for (auto it = ai_data_deque_.rbegin(); it != ai_data_deque_.rend(); ++it) 
        {
            auto ai_data = *it;
            sensor_index += 1;
            long long diff = std::llabs(cur_lidar_data_ptr->time_stamp - ai_data->time_stamp);
            if (diff <= min_diff) 
            {
                min_diff = diff;
                if (diff <= TIME_DIFF) 
                {
                    closest_lidar_data = *cur_lidar_data_ptr;
                    closest_ai_data = *ai_data; 
                    found = true;
                }
                else
                {
                    // std::cout << "AI BAD ERROE TIME INTERVAL IS BIG\n"; 
                    time_interval.push_back(diff);
                    found = false;
                    // return false;
                }
            }
        }

    }
    
    if(!found)
    {
        bool bcondition1 = sensor_type == 1 && time_interval.size() == lidar_data_deque_.size() && time_interval.size() >= 10;
        bool bcondition2 = sensor_type == 2 && time_interval.size() == lidar_data_deque_.size() && time_interval.size() >= 10;
        int index_sensor = 0;
        if(bcondition1 || bcondition2)
        {
            for (int i = time_interval.size() - 1; i >= 1; --i) {
                if (time_interval[i] > TIME_DIFF && time_interval[i-1] > TIME_DIFF) {
                    // return i; // 返回当前下标
                    index_sensor = i;
                    break;
                }
            }

        }

        if(sensor_type == 1 && time_interval.size() >= 15) 
        {
            auto cur_ai_data_ptr = ai_data_deque_.back();
            closest_lidar_data = *lidar_data_deque_[index_sensor];
            closest_ai_data = *cur_ai_data_ptr; 
            found = true;
        }
        else if (sensor_type == 2)
        {
            /* code */
        }
    
    }

    if(found)
    {
        auto cur_ai_timestamp = closest_ai_data.time_stamp;
        while (!ai_data_deque_.empty() && ai_data_deque_.front()->time_stamp <= cur_ai_timestamp) {
            ai_data_deque_.pop_front();  
        }
        auto cur_lidar_timestamp = closest_lidar_data.time_stamp;
        while (!lidar_data_deque_.empty() && lidar_data_deque_.front()->time_stamp <= cur_lidar_timestamp) {
            lidar_data_deque_.pop_front();  
        }
        return true;
    }

    return false;

}